#define _USE_MATH_DEFINES

#include "PVmodel.h"
#include "control.h"

#include <iostream>
#include <fstream>
#include <vector>
#include <cmath>
#include <Eigen/Dense>
#include <Windows.h>

using namespace std;
using namespace Eigen;

#define PI acos(-1.0)

#define TOTAL_TIME 1.0
#define SIMULATION_DELT 2.5E-6

#define NODE_NUM 24
#define BRANCH_NUM 54

//Source
static double G_source;
static double GL_source;

//Load
static double G_load;

//Fault
class Fault
{
public:
    double g_fault;
    double g_on;
    double g_off;
};

//PImodel
class PImodel
{
public:
    double freq, length;
    double Rp_unit, Xp_unit, Xcp_unit;
    double Rz_unit, Xz_unit, Xcz_unit;
    double GC, GCm, G_series_s, G_series_m;
    double R_cal_s, R_cal_m;
    Matrix<double, 6, 6> G_PImodel;

    //有参构造声明
    PImodel(double f, double l, double rp, double xp, double xcp, double rz, double xz, double xcz, double delt);
};

Inductor L_source[3];

//Transformer
static Matrix<double, 2, 2> G_trans;
static Matrix<double, 2, 2> parB_trans;
static Matrix<double, 2, 1> J_trans_A, J_trans_B, J_trans_C;

//PImodel_1
static double Ih_mutual1_1[3], Ih_mutual2_1[3];
static double Ih_self1_1[3], Ih_self2_1[3];
static double Ih_ABC_1[3];

//PImodel_2
static double Ih_mutual1_2[4][3], Ih_mutual2_2[4][3];
static double Ih_self1_2[4][3], Ih_self2_2[4][3];
static double Ih_ABC_2[4][3];

//Y Calculation
static Matrix <double, NODE_NUM, BRANCH_NUM> A_CAL;
static Matrix <double, BRANCH_NUM, NODE_NUM> A_transpose_CAL;
static Matrix <double, BRANCH_NUM, BRANCH_NUM> Yg_CAL;
static Matrix <double, NODE_NUM, BRANCH_NUM> AYg_CAL;

static Matrix <double, NODE_NUM, 1> V_total;

PImodel::PImodel(double f, double l, double rp, double xp, double xcp, double rz, double xz, double xcz, double delt)
{
    freq = f;
    length = l;
    Rp_unit = rp;
    Xp_unit = xp;
    Xcp_unit = xcp;
    Rz_unit = rz;
    Xz_unit = xz;
    Xcz_unit = xcz;

    double invdt = 1 / delt;

    double Rp, Xp, Xcp;
    double Rz, Xz, Xcz;

    Rp = Rp_unit * length;
    Xp = Xp_unit * length;
    Xcp = Xcp_unit / length;
    Rz = Rz_unit * length;
    Xz = Xz_unit * length;
    Xcz = Xcz_unit / length;

    //SI units
    double Rs, Rm, Ls, Lm, Cl, Cg, Cm, C;
    Rs = (Rz + 2.0 * Rp) / 3.0;
    Rm = (Rz - Rp) / 3.0;
    Ls = (Xz / (2.0 * PI * freq) + 2.0 * Xp / (2.0 * PI * freq)) / 3.0;
    Lm = (Xz / (2.0 * PI * freq) - Xp / (2.0 * PI * freq)) / 3.0;
    Cl = (1.0 / (Xcp * 2.0E6 * PI * freq) - 1.0 / (Xcz * 2.0E6 * PI * freq)) / 3.0;
    Cg = 1.0 / (Xcz * 2.0E6 * PI * freq);

    Cm = Cg / 2.0;
    C = Cl / 2.0;

    double R_series_s = Rs + 2 * Ls * invdt;
    double R_series_m = Rm + 2 * Lm * invdt;
    R_cal_s = 2 * Ls * invdt - Rs;
    R_cal_m = 2 * Lm * invdt - Rm;

    G_series_s = (R_series_s * R_series_s - R_series_m * R_series_m)
        / (R_series_s * R_series_s * R_series_s - 3 * R_series_s * R_series_m * R_series_m + 2 * R_series_m * R_series_m * R_series_m);
    G_series_m = R_series_m * (R_series_m - R_series_s)
        / (R_series_s * R_series_s * R_series_s - 3 * R_series_s * R_series_m * R_series_m + 2 * R_series_m * R_series_m * R_series_m);

    //Trapezodial integration of Capacitor
    GC = 2.0 * C * invdt;
    GCm = 2.0 * Cm * invdt;

    double G1 = G_series_s + 2 * GC + GCm;
    double G2 = G_series_m - GC;
    double G3 = -G_series_s;
    double G4 = -G_series_m;

    for(int i = 0; i < 3; i++)
    {
        for(int j = 0; j < 3; j++)
        {
            if(i == j)
            {
                G_PImodel(i, j) = G1;
                G_PImodel(i + 3, j + 3) = G1;
            }
            else
            {
                G_PImodel(i, j) = G2;
                G_PImodel(i + 3, j + 3) = G2;
            }
        }
    }

    for(int i = 0; i < 3; i++)
    {
        for(int j = 0; j < 3; j++)
        {
            if(i == j)
            {
                G_PImodel(i + 3, j) = G3;
                G_PImodel(i, j + 3) = G3;
            }
            else
            {
                G_PImodel(i + 3, j) = G4;
                G_PImodel(i, j + 3) = G4;
            }
        }
    }
}